#include <math.h>
#include <stdbool.h>
#include "HexapodLeg.h"
#include "HexapodRotate.h"
#include "rtthread.h"

// body axis
// MR-hand system unit: mm
const float AxisForeLegX = 51.0f;
const float AxisForeLegY = 101.0f;
const float AxisForeLegZ = 0.0f;

const float AxisMidLegX = 72.5f;
const float AxisMidLegY = 0.0f;
const float AxisMidLegZ = 0.0f;

const float AxisBackLegX = 51.0f; 
const float AxisBackLegY = 95.0f;
const float AxisBackLegZ = 0.0f;

// legs axis mm
const float LegJoint1ToJoint2 = 45.0f;
const float LegJoint2ToJoint3 = 80.0f;
const float LegJoint3ToTip = 145.0f;
const float LegJoint2AngleJoint3 = 23.0f;

// timing setting. unit: ms
const int movementInterval = 5;
const int movementSwitchDuration = 150;
const float K_PI = 3.1415926;

// LegPos locations_operator_sub(const LegPos* locA, const LegPos* locB) {
// 	Point3D FR = opPoint3dSub(&locA->FR, &locB->FR);
// 	Point3D MR = opPoint3dSub(&locA->MR, &locB->MR);
// 	Point3D BR = opPoint3dSub(&locA->BR, &locB->BR);
// 	Point3D BL = opPoint3dSub(&locA->BL, &locB->BL);
// 	Point3D ML = opPoint3dSub(&locA->ML, &locB->ML);
// 	Point3D FL = opPoint3dSub(&locA->FL, &locB->FL);
	
// 	LegPos loc = {FR, MR, BR, BL, ML, FL};
// 	return loc;
// };

// LegPos locations_operator_mul(const LegPos* locA, const float coef) {
// 	Point3D FR = opPoint3dMul(&locA->FR, coef);
// 	Point3D MR = opPoint3dMul(&locA->MR, coef);
// 	Point3D BR = opPoint3dMul(&locA->BR, coef);
// 	Point3D BL = opPoint3dMul(&locA->BL, coef);
// 	Point3D ML = opPoint3dMul(&locA->ML, coef);
// 	Point3D FL = opPoint3dMul(&locA->FL, coef);
	
// 	LegPos loc = {FR, MR, BR, BL, ML, FL};
// 	return loc;
// };

// LegPos locations_operator_plus(const LegPos* locA, const LegPos* locB) {
// 	Point3D FR = opPoint3dPlus(&locA->FR, &locB->FR);
// 	Point3D MR = opPoint3dPlus(&locA->MR, &locB->MR);
// 	Point3D BR = opPoint3dPlus(&locA->BR, &locB->BR);
// 	Point3D BL = opPoint3dPlus(&locA->BL, &locB->BL);
// 	Point3D ML = opPoint3dPlus(&locA->ML, &locB->ML);
// 	Point3D FL = opPoint3dPlus(&locA->FL, &locB->FL);
	
// 	LegPos loc = {FR, MR, BR, BL, ML, FL};
// 	return loc;
// };


void translateBodyToLocal(const Point3D* body, Point3D* local, Leg* leg) {
	Point3D pt3d = opPoint3dSub(body, &leg->extrinsics);
	leg->localConv(&pt3d, local);
}

void translateLocalToBody(const Point3D* local, Point3D* body, Leg* leg) {
	leg->bodyConv(local, body);
	Point3D pt3d = opPoint3dPlus(body, &leg->extrinsics);
	body->x = pt3d.x;
	body->y = pt3d.y;
	body->z = pt3d.z;
}

void forwardKinematics(const float angle[3], Point3D* dst) {
	float radian[3];
	for(int i=0; i<3; i++) {
			radian[i] = K_PI * angle [i] / 180.0f;
	}
	float joint2radian= LegJoint2AngleJoint3 * K_PI / 180.0f;
	float Lxy = LegJoint1ToJoint2 + cos(joint2radian - radian[1]) * LegJoint2ToJoint3 + cos(0.5 * K_PI - radian[1] - radian[2]) * LegJoint3ToTip;
	dst->x = cos(radian[0]) * Lxy;
	dst->y = sin(radian[0]) * Lxy;
	dst->z = sin(joint2radian - radian[1]) * LegJoint2ToJoint3 + cos( - radian[1] - radian[2]) * LegJoint3ToTip;
}

void inverseKinematics(const Point3D* dst, float angles[3]) {
	float x = dst->x;
	float y = dst->y;
	angles[0] = atan2(y, x) * 180 / K_PI;
	x = sqrt(x*x + y*y) - LegJoint1ToJoint2;
	y = dst->z;
	float ar = atan2(y, x);
	float lr2 = x * x + y * y;
	float lr = sqrt(lr2);
	float a1 = acos((lr2 + LegJoint2ToJoint3 * LegJoint2ToJoint3 - LegJoint3ToTip * LegJoint3ToTip) / (2 * LegJoint2ToJoint3 * lr));
	float a2 = acos((lr2 - LegJoint2ToJoint3 * LegJoint2ToJoint3 + LegJoint3ToTip * LegJoint3ToTip) / (2 * LegJoint3ToTip * lr));
	angles[1] = LegJoint2AngleJoint3 - (ar - a1) * 180 / K_PI;
	angles[2] = 90.0f - LegJoint2AngleJoint3 - ((a1 + a2)  * 180.0f / K_PI);
}


void legInitHelp(Leg* leg, const Point3D pt3d) {
	leg->extrinsics.x = pt3d.x;
	leg->extrinsics.y = pt3d.y;
	leg->extrinsics.z = pt3d.z;
}

// body axis
void legInit(uint8_t index, Leg* leg) {
	switch(index) {
		case 0: { // 45 degree 
			Point3D pt3d = {AxisForeLegX, AxisForeLegY, AxisForeLegZ};
			legInitHelp(leg, pt3d);
			leg->localConv = rotate315;
			leg->bodyConv = rotate45;
		} break;
		case 1: {// 0 degree
			Point3D pt3d = {AxisMidLegX, AxisMidLegY, AxisMidLegZ};
			legInitHelp(leg, pt3d);
			leg->localConv = rotate0;
			leg->bodyConv = rotate0;
		} break;
		case 2:{ // -45 or 315 degree
			Point3D pt3d = {AxisBackLegX, -AxisBackLegY, AxisBackLegZ};
			legInitHelp(leg, pt3d);
			leg->localConv = rotate45;
			leg->bodyConv = rotate315;
		} break;
		case 3:{ // -135 or 225 degree
			Point3D pt3d = {-AxisBackLegX, -AxisBackLegY, AxisBackLegZ};
			legInitHelp(leg, pt3d);
			leg->localConv = rotate135;
			leg->bodyConv = rotate225;
		} break;
		case 4: {// 180 degree
			Point3D pt3d = {-AxisMidLegX, AxisMidLegY, AxisBackLegZ};
			legInitHelp(leg, pt3d);
			leg->localConv = rotate180;
			leg->bodyConv = rotate180;
		} break;
		case 5: {// 135 degree
			Point3D pt3d = {-AxisForeLegX, AxisForeLegY, AxisForeLegZ};
			legInitHelp(leg, pt3d);
			leg->localConv = rotate225;
			leg->bodyConv = rotate135;
		} break;
		default: break;
	}
	
	float angle[3] = {0};
	for(uint8_t i = 0; i < 3; i++) {
		leg->servo[i].id = SERVOS[index * 3 + i].id;
		leg->servo[i].type = SERVOS[index * 3 + i].type;
		leg->servo[i].angle = SERVOS[index * 3 + i].angle;
		leg->servo[i].angleOffset = SERVOS[index * 3 + i].angleOffset;
		leg->servo[i].second = SERVOS[index * 3 + i].second;
		leg->servo[i].maxAngle = SERVOS[index * 3 + i].maxAngle;
		leg->servo[i].maxTicks = SERVOS[index * 3 + i].maxTicks;
		leg->servo[i].direction = SERVOS[index * 3 + i].direction;
		
		angle[i] = SERVOS[index * 3 + i].angle;
		setServoAngle(&leg->servo[i]);
	}		
	Point3D local, body;
	forwardKinematics(angle, &local);
	translateLocalToBody(&local, &body, leg);
	leg->localPos = local;
	leg->bodyPos = body;
}

void setLegAngle(float angle[3], Leg* leg) {
	Point3D dst;
	forwardKinematics(angle, &dst);
	setLocalTip(&dst, leg);
//	for(uint8_t i = 0; i < 3; i++) {
//		leg->servo[2-i].angle = angle[i];
//		setServoAngle(&leg->servo[2-i]);
//	}	
}

void setLegPosition(const Point3D* dst, Leg* leg) {
	float angle[3];
	inverseKinematics(dst, angle);
	for(uint8_t i = 0; i < 3; i++) {
		leg->servo[2-i].angle = angle[i];
		setServoAngle(&leg->servo[2-i]);
	}	
}

void setBodyTip(const Point3D* body, Leg* leg) {
	bool rnt = opPoint3dEqual(body, &leg->bodyPos);
	if (rnt == true) {
			return;
	}
	Point3D local;    
	translateBodyToLocal(body, &local, leg);
	setLegPosition(&local, leg);
	leg->localPos = local;
	leg->bodyPos = *body;
}

void setLocalTip(const Point3D* local, Leg* leg) {
	bool rnt = opPoint3dEqual(local, &leg->localPos);
	if (rnt == true) {
			return;
	}	
	Point3D body;
	translateLocalToBody(local, &body, leg);
	setLegPosition(local, leg);
	//leg->localPos = *local;
	//leg->bodyPos = body;
}

void test_leg_move() {
	// init
	Leg Global_leg[6] = {0};
	for (uint8_t i = 0; i < 6; i++) {
		legInit(i, &Global_leg[i]);
	}
	rt_thread_delay(2000);//2000ms
	// test move
//	Point3D dst = {30, 30, 10};
	float angle[3] = {0, 23, 67};
	setLegAngle(angle, &Global_leg[0]);
	//setLegPosition(&dst, &leg[0]);
}

		
		
		
		
		
		
		
		
		
		
		
		
		
		
		
		
		
		
		
		
		
		
		
		
		
		
		